hip.math.collision source code

1 module hip.math.collision;
2 import hip.math.utils:sqrt;
3 import std.traits:isNumeric;
4 public import hip.math.rect;
5 public import hip.math.vector;
6 
7 
8 pure nothrow @nogc @safe:
9 
10 bool isPointInCircle(T)(in T px, in T py, in T circleX, in T circleY, in T circleRadius) 
11 if(isNumeric!T)
12 {
13     float dx = px-circleX;
14     float dy = py-circleY;
15     return sqrt(dx*dx+dy*dy) <= circleRadius;
16 }
17 
18 bool isPointInCircle(in Vector2 point, in Vector2 circle, in float radius)
19 {
20     return (circle - point).mag <= radius;
21 }
22 
23 
24 bool isPointInRect(T)(in T px, in T py, in T rx, in T ry, in T rw, in T rh)
25 if(isNumeric!T)
26 {
27     return !(px <= rx || px >= rx+rw || py <= ry || py >= ry+rh);
28 }
29 bool isPointInRect(in Vector2 p, in Rect r)
30 {
31     return !(p.x <= r.x || p.x >= r.x+r.w || p.y <= r.y || p.y >= r.x+r.h);
32 }
33 
34 
35 
36 ///Error AKA approximation
37 bool isPointInLine(T)(in T px, in T py, in T lx1, in T ly1, in T lx2, in T ly2, in float error = 0.01)
38 if(isNumeric!T)
39 {
40     import hip.math.utils;
41     float lineLength = (lx2 - lx1)^^2 +(ly2 - ly1)^^2;
42     float distLeft = (lx1 - px)^^2 + (ly1 - py)^^2;
43     float distRight = (lx2 - px)^^2 + (ly2 - py)^^2;
44 
45     return sqrt(lineLength).approximatelyEqual(sqrt(distLeft)+sqrt(distRight), error);
46 }
47 bool isPointInLine2(T)(in T px, in T py, in T lx1, in T ly1, in T lx2, in T ly2, in float error = 0.01)
48 if(isNumeric!T)
49 {
50     import hip.math.utils;
51 
52     if(px < lx1 || px > lx2) return false;
53 
54     float dx = lx2 - lx1;
55     float dy = ly2 - ly1;
56 
57     float slope = dy/dx;
58     float yIntercept = ly1 - (slope*lx1);
59 
60     return py.approximatelyEqual(slope*px + yIntercept, error);
61 }
62 
63 pragma(inline, true)
64 bool isPointInLine(T)(in T[2] point, in T[2] lineStart, in T[2] lineEnd, in float error = 0.01)
65 {
66     return isPointInLine(point[0], point[1], lineStart[0], lineStart[1], lineEnd[0], lineEnd[1], error);
67 }
68 pragma(inline, true)
69 bool isPointInLine(in Vector2 point, in Vector2 lineStart, in Vector2 lineEnd, in float error = 0.01)
70 {
71     return isPointInLine(point[0], point[1], lineStart[0], lineStart[1], lineEnd[0], lineEnd[1], error);
72 }
73 
74 bool isCircleInLine(float circleX, float circleY, float radius, float lx1, float ly1, float lx2, float ly2)
75 {
76     if(isPointInCircle(lx1, ly1, circleX, circleY, radius) || isPointInCircle(lx2, ly2, circleX, circleY, radius))
77         return true;
78     import hip.math.utils;
79     float lineDistX = lx2 - lx1;
80     float lineDistY = ly2 - ly1;
81     float lineLengthSquared = lineDistX*lineDistX + lineDistY*lineDistY;
82 
83     //Dot product between circle pos distance to line start to line vector.
84     //Remember dot is the same as length squared vector, so need to divide by squared length to normalize.
85     float dot = ((circleX - lx1) * lineDistX + (circleY - ly1) * lineDistY) / lineLengthSquared;
86 
87     ///Closest point from the circleX and Y to the line
88     float closestX = lx1 + dot*lineDistX, closestY = ly1 + dot*lineDistY;
89     if(!isPointInLine(closestX, closestY, lx1, ly1, lx2, ly2))
90         return false;
91 
92     //Returns if distance <= radius from the closest point in line to the circle point
93     return sqrt((closestX - circleX)^^ 2 + (closestY - circleY)^^ 2) <= radius;
94 }
95 
96 bool isRayIntersectingLine(in Vector2 l1Start, in Vector2 l1End, in Vector2 l2Start, in Vector2 l2End, out Vector2 intersection)
97 {
98     Vector2 line1 = l1End - l1Start;
99     Vector2 line2 = l2End - l2Start;
100     return false;
101 }
102 
103 
104 bool isRayIntersectingRect(in Vector2 rayPos, in Vector2 rayEnd, in Rect rect, out Vector2 intersection, out Vector2 intersectionNormal, out float intersectionTime)
105 {
106     import hip.math.utils;
107 
108     Vector2 rayDir = rayEnd - rayPos;
109     if(rayDir.magSquare == 0)
110         return false;
111     //T when hitting the near point to rayPos in X axis
112 
113     Vector2 recStart = Vector2(rect.x, rect.y);
114     Vector2 recSize = Vector2(rect.w, rect.h);
115 
116     Vector2 nearT = (recStart - rayPos);
117     if((nearT.x == 0 && rayDir.x == 0) || (nearT.y == 0 && rayDir.y == 0)) //Prevents NaN
118         return false;
119     nearT/= rayDir;
120 
121     Vector2 farT = (recStart + recSize - rayPos);
122     if((farT.x == 0 && rayDir.x == 0) || (farT.y == 0 && rayDir.y == 0)) //Prevents NaN
123         return false;
124     farT/= rayDir;
125 
126     auto swap = (ref float a, ref float b)
127     {
128         float temp = b;
129         b = a;
130         a = temp;
131     };
132 
133     if(nearT.x > farT.x) swap(nearT.x, farT.x);
134     if(nearT.y > farT.y) swap(nearT.y, farT.y);
135     //No collision
136     if(nearT.x > farT.y || nearT.y > farT.x) return false;
137 
138 
139     float tHitNear = max(nearT.x, nearT.y);
140     float tHitFar = min(farT.x, farT.y);
141 
142     //Collision in opposite direction
143     intersectionTime = tHitNear;
144     if(tHitFar < 0 || tHitNear < 0) return false;
145 
146     intersection = rayPos + tHitNear * rayDir;
147 
148     //Hit on top of the rect
149     if(nearT.x > nearT.y)
150     {
151         if(rayDir.x > 0)
152             intersectionNormal = Vector2(-1, 0);
153         else
154             intersectionNormal = Vector2(1, 0);
155     }
156     //Hit on bottom of the rect
157     else
158     {
159         if(rayDir.y > 0)
160             intersectionNormal = Vector2(0, -1);
161         else
162             intersectionNormal = Vector2(0, 1);
163     }
164     return true;
165 }
166 
167 /**
168 *   Automatically updates Rect velocity if collision happened. If you wish a non changing velocity, send a velocity copy.
169 */
170 bool isDynamicRectOverlappingRect(in Rect source, in Vector2 vel, in Rect target, in float deltaTime, out Vector2 intersectionNormal, out float intersectionTime)
171 {
172     if(vel.x == 0 && vel.y == 0)
173         return false;
174 
175     float w = cast(float)source.w;
176     float h = cast(float)source.h;
177     Rect expandedTarget = Rect(target.x-w, target.y-h, target.w+w, target.h+h);
178 
179     Vector2 intersection = void;
180     if(isRayIntersectingRect(source.position, source.position + vel*deltaTime, expandedTarget, intersection, intersectionNormal, intersectionTime) 
181         && intersectionTime <= 1.0)
182         return true;
183     return false;
184 }
185 
186 void resolveDynamicRectOverlappingRect(in Vector2 normal, ref Vector2 velocity, in float intersectionTime)
187 {
188     import hip.math.utils:abs;
189     velocity+= normal * Vector2(velocity.x.abs, velocity.y.abs) * (1-intersectionTime);
190 }
191 
192 
193 bool isRectOverlappingRect(in Rect r1, in Rect r2)
194 {
195     const float r1x2 = r1.x+r1.w;
196     const float r2x2 = r2.x+r2.w;
197     const float r1y2 = r1.y+r1.h;
198     const float r2y2 = r2.y+r2.h;
199     return !(r1x2 < r2.x || r1.x > r2x2 || r1y2 < r2.y || r1.y > r2y2);
200 }
201 
202 struct RectWorld
203 {
204     private DynamicRect[] dynamicRects;
205     private Rect[] staticRects;
206 
207     /**
208     *   Returns a reference to the dynamic rect as they will need to be manipulated.
209     */
210     DynamicRect* addDynamic(in Rect rect, in Vector2 velocity)
211     {
212         dynamicRects~= DynamicRect(rect, velocity);
213         return &dynamicRects[$-1];
214     }
215 
216 
217     /**
218     *   Should never be manipulated, which is why their reference is not returned.
219     */
220     void addStatic(in Rect[] rect...)
221     {
222         foreach(r;rect)
223             staticRects~= r;
224     }
225 
226     void update(float dt)
227     {
228         struct DynamicRectCollision
229         {
230             Vector2 normal;
231             float time;
232         }
233         foreach(ref DynamicRect dynamic; dynamicRects)
234         {
235             scope DynamicRectCollision[] collisionList;
236             foreach(const ref rect; staticRects)
237             {
238                 DynamicRectCollision col = void;
239                 if(isDynamicRectOverlappingRect(dynamic.rect, dynamic.velocity, rect, dt, col.normal, col.time))
240                 {
241                     collisionList~= col;
242                 }
243             }
244             if(collisionList.length > 0)
245             {
246                 import std.algorithm.sorting:sort;
247                 foreach(col; sort!((DynamicRectCollision a, DynamicRectCollision b) => a.time < b.time)(collisionList))
248                 {
249                     resolveDynamicRectOverlappingRect(col.normal, dynamic.velocity, col.time);
250                 }
251             }
252             destroy(collisionList);
253             dynamic.move(dynamic.velocity * dt);
254         }
255     }
256 
257 
258     @system int opApply(scope int delegate(ref DynamicRect) dg)
259     {
260         int result = 0;
261         foreach (ref DynamicRect item; dynamicRects)
262         {
263             result = dg(item);
264             if (result)
265                 break;
266         }
267         return result;
268     }
269 
270     @system int opApply(scope int delegate (const ref Rect) dg)
271     {
272         int result = 0;
273         foreach (const ref Rect item; staticRects)
274         {
275             result = dg(item);
276             if (result)
277                 break;
278         }
279         return result;
280     }
281 }